package nav;

import tools.Fast;


/**Navigator is a thread that controls the motion of the robot.
 * Waits for setMotion to be called and executes the required motion.
 * Has options for immediate return.
 * Forward and rotational speeds in cm / s and degrees / s, respectively
 * @author Antoine Lennartz*/
public class Navigator extends Thread{
	
	/**Default values for forward and rotation speeds*/
	public final double FORWARD_SPEED=7, ROTATION_SPEED=30;
	/**error tolerance for final position*/
	public final double FORWARD_TOLERANCE=1, ROTATION_TOLERANCE=Fast.toRad(1);
	/**link to main Odometer*/
	private Odometer odo;
	/**link to base: wheels etc*/
	private TwoWheeledRobot robot;
	/**link to speed controller*/
	private SpeedController sc;	
	/**status for navigator*/
	private boolean navigating=false;
	/**current target*/
	private double[] target = new double[3];
	/**allow or not reverse motion*/
	private boolean allowReverse=false;
	
	/**Constructor initializes */
	public Navigator(Odometer odo){
		this.odo = odo;
		this.robot = odo.getTwoWheeledRobot();
		sc=new SpeedController(odo);
		this.start();
	}
	
	public void turnTo(double angle, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		turn (Fast.fixAngle2(angle-cPos[2]), immediateReturn);
	}
	
	/**Converts the set motion to target coordinate and then tell the captain that it has a motion to perform*/
	public void turn(double angle, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		target[0]=cPos[0];
		target[1]=cPos[1];
		target[2]=angle;
		allowReverse=false;
		this.interrupt();
		while(!robot.isMoving() && Math.abs(angle)>ROTATION_TOLERANCE)Thread.yield();
		if(immediateReturn) return;
		while(robot.isMoving()) Thread.yield();
	}
	
	/**Converts the set motion to target coordinate and then tell the captain that it has a motion to perform
	 * @param dist - distance to travel
	 * @param immediateReturn - if false does not return thread till motion is complete*/

	public void move(double dist, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		target[0]=cPos[0]+dist*Math.sin(cPos[2]);
		target[1]=cPos[1]+dist*Math.cos(cPos[2]);
		target[2]=cPos[2];
		allowReverse=true;//allowReverse=true;
		//Thread.yield();
		this.interrupt();
		while(!robot.isMoving() && Math.abs(dist)>FORWARD_TOLERANCE)Thread.yield();
		if(immediateReturn) return;
		while(robot.isMoving()) Thread.yield();
	}
	/**Converts the set motion to target coordinate and then tell the captain 
	 * that it has a motion to perform
	 * @param target - target position
	 * @param immediateReturn returns immediately or not from this method*/

	public void moveTo(double[] target, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		this.target[0]=target[0];
		this.target[1]=target[1];
		if(target.length==2)
			this.target[2]= Fast.atan3(target[0]-cPos[0], target[1]-cPos[1]);
		else 
			this.target[2]=Fast.fixAngle(target[2]);
		allowReverse=false;
		Thread.yield();
		this.interrupt();
		while(!robot.isMoving() && ( Fast.distance(target, cPos)>FORWARD_TOLERANCE || Math.abs(Fast.fixAngle2(target[2]-cPos[2]))>ROTATION_TOLERANCE ))Thread.yield();
		if(immediateReturn) return;
		while(robot.isMoving()) Thread.yield();
	}
	
	/**Sleeps the Navigator until SetMotion is called, then it performs the motion
	 * and goes back to sleep*/
	public void run() {
		while(true){
			try {
				Thread.sleep(60000);
			} catch (InterruptedException e) {
				navigating=true;
				moveToTarget();
				navigating=false;
			}
		}
	}
	
	
	/**performs the motion to the current immediate target*/
	private void moveToTarget(){
		double[] scales= new double[2];		
		robot.stop();
		sc.setTargPos(target);
		sc.setEnabled(true);
		sc.setAllowReverse(allowReverse);
		
		double dist=sc.getDist(), dAng=sc.getdAng();
		//double sleepFactor = 2*Math.log(dist+3)*Math.log(dist+3);
				
		//To Target
		while(Math.abs(dist)>FORWARD_TOLERANCE || Math.abs(dAng)>ROTATION_TOLERANCE){
			//sleepFactor = 2*Math.log(dist+3)*Math.log(dist+3);
			sc.getScales(scales);
			robot.setSpeeds(scales[0]*FORWARD_SPEED, scales[1]*ROTATION_SPEED);
			//try {Thread.sleep((long) (Odometer.DEFAULT_PERIOD*sleepFactor));} catch (InterruptedException e) {}
			dist=sc.getDist();
			dAng=sc.getdAng();
		}
		robot.stop();
		
		//To Angle
		/*dAng=sc.getdAng();	
		if(Math.abs(dAng)>10)robot.setSpeeds(0, Fast.signOf(dAng)*ROTATION_SPEED);
		while(){
			sleepFactor = 5*Math.log(dist+3);
			
			try {Thread.sleep((long) (Odometer.DEFAULT_PERIOD*sleepFactor));} catch (InterruptedException e) {}
		}
		
		dAng=sc.getdAng();
		while(Math.abs(dAng)>ROTATION_TOLERANCE){
			sleepFactor = 5*Math.log(dist+3);
			sc.getAngleScales(scales);
			robot.setSpeeds(0, scales[1]*ROTATION_SPEED);
			try {Thread.sleep((long) (Odometer.DEFAULT_PERIOD*sleepFactor));} catch (InterruptedException e) {}
			dAng=sc.getdAng();
		}
		robot.stop();*/
		sc.setEnabled(false);
	}	

	/**@return true if path underway or pallet pickup in progress(that's not intended)*/
	public boolean isNavigating(){
		return navigating;
	}
	
	
}
